#include <physics/motionstate.h>
#include <assert.h>


cmEntityMotionState::cmEntityMotionState(ceEntityNode* entity, btRigidBody* body)
  : btMotionState ()
  , _entity (0)
  , _body (0)
{

  SetEntity(entity);
  SetRigidBody(body);
}


void cmEntityMotionState::SetEntity(ceEntityNode *entity)
{
  CE_SET(_entity, entity);
}

ceEntityNode* cmEntityMotionState::GetEntity()
{
  return _entity;
}

const ceEntityNode* cmEntityMotionState::GetEntity() const
{
  return _entity;
}

void cmEntityMotionState::SetRigidBody(btRigidBody *body)
{
  _body = body;
}

btRigidBody* cmEntityMotionState::GetRigidBody()
{
  return _body;
}

const btRigidBody* cmEntityMotionState::GetRigidBody() const
{
  return _body;
}

void cmEntityMotionState::setWorldTransform(const btTransform &worldTrans)
{
  assert (_entity);
  ceMatrix4f mat;
  worldTrans.getOpenGLMatrix(&mat._00);

  _entity->SetMatrix(mat);
  _entity->FinishTransformation(false);
}

void cmEntityMotionState::getWorldTransform(btTransform &worldTrans) const
{
  assert (_entity);
  worldTrans.setFromOpenGLMatrix(&_entity->GetMatrix()._00);
}

